// 引入必要的ROS 2头文件
#include "rclcpp/rclcpp.hpp"                    // ROS 2的核心功能
#include "turtlesim/msg/pose.hpp"               // 乌龟位置消息类型
#include "geometry_msgs/msg/twist.hpp"          // 速度消息类型
#include <cmath>                                // 数学函数

// 使用命名空间
using namespace std::chrono_literals;           // 时间字面量

// 定义乌龟信息打印节点类，继承自rclcpp::Node
class TurtleInfoPrinter : public rclcpp::Node
{
public:
    // 构造函数
    TurtleInfoPrinter() : Node("turtle_info_printer")  // 设置节点名
    {
        // 声明参数：要订阅的乌龟名称
        this->declare_parameter<std::string>("turtle_name", "turtle1");
        turtle_name_ = this->get_parameter("turtle_name").as_string();
        
        // 构建话题名称
        std::string pose_topic = "/" + turtle_name_ + "/pose";
        
        // 创建位置信息订阅者
        pose_subscriber_ = this->create_subscription<turtlesim::msg::Pose>(
            pose_topic, 10, std::bind(&TurtleInfoPrinter::pose_callback, this, std::placeholders::_1));
        
        // 创建速度信息订阅者（可选）
        std::string cmd_vel_topic = "/" + turtle_name_ + "/cmd_vel";
        cmd_vel_subscriber_ = this->create_subscription<geometry_msgs::msg::Twist>(
            cmd_vel_topic, 10, std::bind(&TurtleInfoPrinter::cmd_vel_callback, this, std::placeholders::_1));
        
        // 创建定时器，定期打印摘要信息
        timer_ = this->create_wall_timer(
            2000ms, std::bind(&TurtleInfoPrinter::timer_callback, this));
        
        // 初始化变量
        last_pose_time_ = this->now();
        total_distance_ = 0.0;
        last_x_ = 0.0;
        last_y_ = 0.0;
        first_pose_received_ = false;
        
        RCLCPP_INFO(this->get_logger(), 
                   "乌龟信息打印节点已启动，正在监听乌龟: %s", 
                   turtle_name_.c_str());
        RCLCPP_INFO(this->get_logger(), 
                   "位置话题: %s", pose_topic.c_str());
    }

private:
    // 成员变量
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_subscriber_;      // 位置订阅者
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscriber_; // 速度订阅者
    rclcpp::TimerBase::SharedPtr timer_;                     // 定时器
    
    std::string turtle_name_;                   // 乌龟名称
    turtlesim::msg::Pose current_pose_;         // 当前位置
    geometry_msgs::msg::Twist current_cmd_vel_; // 当前速度命令
    
    rclcpp::Time last_pose_time_;               // 上次接收位置时间
    double total_distance_;                     // 总移动距离
    double last_x_;                             // 上次x坐标
    double last_y_;                             // 上次y坐标
    bool first_pose_received_;                  // 是否收到第一个位置信息
    
    // 位置信息回调函数
    void pose_callback(const turtlesim::msg::Pose::SharedPtr msg)
    {
        // 存储当前位置
        current_pose_ = *msg;
        
        // 计算移动距离（如果是第一次接收位置，不计算距离）
        if (!first_pose_received_) {
            first_pose_received_ = true;
            last_x_ = msg->x;
            last_y_ = msg->y;
        } else {
            double dx = msg->x - last_x_;
            double dy = msg->y - last_y_;
            double distance = std::sqrt(dx * dx + dy * dy);
            total_distance_ += distance;
            
            last_x_ = msg->x;
            last_y_ = msg->y;
        }
        
        last_pose_time_ = this->now();
        
        // 实时打印位置信息
        RCLCPP_INFO(this->get_logger(), 
                   "=== 乌龟位置信息 ===");
        RCLCPP_INFO(this->get_logger(), 
                   "坐标: (%.2f, %.2f)", msg->x, msg->y);
        RCLCPP_INFO(this->get_logger(), 
                   "方向: %.2f 弧度 (%.2f 度)", 
                   msg->theta, msg->theta * 180.0 / M_PI);
        RCLCPP_INFO(this->get_logger(), 
                   "线速度: %.2f, 角速度: %.2f", 
                   msg->linear_velocity, msg->angular_velocity);
        RCLCPP_INFO(this->get_logger(), 
                   "累计移动距离: %.2f 米", total_distance_);
    }
    
    // 速度命令回调函数（可选）
    void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
    {
        current_cmd_vel_ = *msg;
        
        // 只在有实际速度时打印
        if (msg->linear.x != 0.0 || msg->angular.z != 0.0) {
            RCLCPP_DEBUG(this->get_logger(), 
                        "速度命令 - 线速度: %.2f, 角速度: %.2f", 
                        msg->linear.x, msg->angular.z);
        }
    }
    
    // 定时器回调函数，定期打印摘要
    void timer_callback()
    {
        auto current_time = this->now();
        auto time_since_last_pose = current_time - last_pose_time_;
        
        RCLCPP_INFO(this->get_logger(), 
                   "=== 乌龟状态摘要 ===");
        RCLCPP_INFO(this->get_logger(), 
                   "乌龟名称: %s", turtle_name_.c_str());
        RCLCPP_INFO(this->get_logger(), 
                   "当前位置: (%.2f, %.2f)", 
                   current_pose_.x, current_pose_.y);
        RCLCPP_INFO(this->get_logger(), 
                   "当前方向: %.2f 弧度", current_pose_.theta);
        RCLCPP_INFO(this->get_logger(), 
                   "累计移动距离: %.2f 米", total_distance_);
        RCLCPP_INFO(this->get_logger(), 
                   "距离上次位置更新: %.1f 秒", 
                   time_since_last_pose.seconds());
        
        if (time_since_last_pose.seconds() > 5.0) {
            RCLCPP_WARN(this->get_logger(), 
                       "已有 %.1f 秒未收到位置更新", 
                       time_since_last_pose.seconds());
        }
    }
};

// 主函数
int main(int argc, char * argv[])
{
    // 初始化ROS 2
    rclcpp::init(argc, argv);
    
    // 创建节点并进入循环处理
    auto node = std::make_shared<TurtleInfoPrinter>();
    
    // 保持节点运行，直到接收到终止信号
    rclcpp::spin(node);
    
    // 关闭ROS 2
    rclcpp::shutdown();
    
    return 0;
}